#ifndef _LSPB_H
#define _LPSB_H    1

#include "utils.h"

void Normal_LSPB(char *cmds)
{
	const int LineNum = wc_l("多轴的直线路径轨迹.txt");

	/* 读取轨迹 */
	double q[LineNum][6];

	FILE *ifp = fopen("多轴的直线路径轨迹.txt", "r");
	char buf[100];
	for (int i=0; i<LineNum; i++)
	{
		for (int j=0; j<6; j++)
		{
			fscanf(ifp, "%s", buf);
			q[i][j] = atof(buf);
		}
	}
	fclose(ifp);
	
	/* 设置加速时间 */
	double AcceTime[LineNum];
	AcceTime[0] = 0.125;
	AcceTime[1] = 0.133;
	for (int i=2; i<LineNum-1; i++)
	{
		AcceTime[i] = 0.016;
	}
	AcceTime[LineNum-1] = 0.016;

	/* 设置路径段持续时间 */
	double duration[LineNum-1];
	duration[0] = 0.4415;
	duration[1] = 0.0905;
	for (int i=2; i<LineNum-2; i++)
	{
		duration[i] = 0.032;
	}
	duration[LineNum-2] = 0.032;

	/* 计算直线区段时间, 速度和过渡段加速度 */
	double LineTime[LineNum-1][6];
	double v[LineNum-1][6];
	double a[LineNum][6];
	for (int k=0; k<6; k++)
	{
		/* 直线区段时间 */
		LineTime[0][k] = duration[0] - AcceTime[0] - 0.5 * AcceTime[1];
		for (int i=1; i<LineNum-2; i++)
		{
			LineTime[i][k] = duration[i] - 0.5 * AcceTime[i] - 0.5 * AcceTime[i+1];
		}
		LineTime[LineNum-2][k] = duration[LineNum-2] - 0.5 * AcceTime[LineNum-2] - AcceTime[LineNum-1];
		/* check */
		for (int i=0; i<LineNum-1; i++)
		{
			assert(LineTime[i][k] >= 0);
		}

		/* 直线区段速度 */
		v[0][k] = (q[1] - q[0]) / (duration[0] - 0.5 * AcceTime[0]);
		for (int i=1; i<LineNum-2; i++)
		{
			v[i][k] = (q[i+1] - q[i]) / duration[i];
		}
		v[LineNum-2][k] = (q[LineNum-1] - q[LineNum-2]) / (duration[LineNum-2] - 0.5 * AcceTime[LineNum-1]);
		
		/* 过渡段加速度 */
		a[0][k] = (v[0][k] - 0) / AcceTime[0];
		for (int i=1; i<LineNum-1; i++)
		{
			a[i][k] = (v[i][k] - v[i-1][k]) / AcceTime[i];
		}
		a[LineNum-1][k] = (0 - v[LineNum-2][k]) / AcceTime[LineNum-1];
		/* check */
		for (int i=0; i<LineNum; i++)
		{
			assert(fabs(a[i][k]) <= 5);
		}
	}

	/* 计算主轴加速度 */
	double main_axis_a[LineNum];
	double temp;
	for (int i=0; i<LineNum; i++)
	{
		temp = 0;
		for (int j=0; j<6; j++)
		{
			if (fabs(a[i][j]) > temp)
			{
				main_axis_a[i] = a[i][j];
				temp = fabs(a[i][j]);
			}
		}
	}

	/* 生成speedj命令程序 */
	sprintf(cmds, "def move():\n");
	
	for (int i=0; i<LineNum-1; i++)
	{
		sprintf(cmds+strlen(cmds), "speedj([%lf,%lf,%lf,%lf,%lf,%lf],a=%lf,t=%lf)\n", v[i][0],v[i][1],v[i][2],v[i][3],v[i][4],v[i][5],main_axis_a[i], AcceTime[i] + LineTime[i][0]);
	}
	sprintf(cmds+strlen(cmds), "speedj([%lf,%lf,%lf,%lf,%lf,%lf],a=%lf,t=%lf)\n", v[LineNum-1][0],v[LineNum-1][1],v[LineNum-1][2],v[LineNum-1][3],v[LineNum-1][4],v[LineNum-1][5], main_axis_a[LineNum-1], AcceTime[LineNum-1]);

	sprintf(cmds+strlen(cmds), "end\n");
}

#endif /* <lspb.h> included.  */